#pragma once
#include <deque>
#include <stack>
#include <set>
#include <algorithm>
#include <functional>
#include <cassert>
#include "PacManClone.h"
#include "Object.h"
#include "AStar.h"
#include "Ghost_ai.h"
#include <iostream>

using namespace std;

template <class Node>
AStarNode<Node>::AStarNode( )
: node(), parent(NULL), h(0), g(0)
{
}

template <class Node>
AStarNode<Node>::AStarNode( const Node &n )
: node(n), parent(NULL), h(0), g(0)
{
}

template <class Node>
AStarNode<Node>::AStarNode( const Node &n, AStarNode<Node> *nodesParent, const int _h, const int _g )
: node(n), parent(nodesParent), h(_h), g(_g), f(_h + _g)
{
}

template <class Node>
int AStarNode<Node>::getF( ) const
{ 
	return f;
}


template <class Node>
bool less_AStar_f<Node>::operator()( const AStarNode<Node> *a1, const AStarNode<Node> *a2 ) const
{ 
	return a1->getF( ) > a2->getF( );

}

	



AStar::AStar( )
 : m_Allocator( ), openList( ), closeList( ), bPathFound(false), startOfPath( )
{
}

AStar::~AStar( )
{
}



//	
//	Add start node to Open list
//
//	Do:
//		Search for node on Open that has best estimate (cost + heuristic)
//		Move that node to Closed list
//		If node is goal, break with success
//		Else, test all neighboring nodes that can be reached from there
//			Calculate estimate (cost + heuristic) for each node
//			If node is already on Open
//				If estimate is better than current, keep it on Open
//				Else, move it to Closed (it's worse, so we can ignore it)
//			If node is already on Closed
//				If estimate is better than current, move it back to Open
//				Else, keep it on Closed
//			If node is not on either list, put it on Open
//	Loop while Open has nodes on it
//	
bool AStar::computeShortestPath( Object *pStartNode, Object *pEndNode )
{
	less_AStar_f<Object *> less_f;

	make_heap( openList.begin( ), openList.end( ), less_f ); // create a binary min heap

	AStarNode<Object *> *startNode = m_Allocator.allocate( 1 );
	m_Allocator.construct( startNode, AStarNode<Object *>( pStartNode, NULL, H(pStartNode, pEndNode), 0 ) );

	openList.push_back( startNode );
	push_heap( openList.begin( ), openList.end( ), less_f ); // push onto a binary min heap...

	bPathFound = false;

	while( openList.size( ) > 0 && !bPathFound )
	{
		AStarNode<Object *> *current = openList.front( );

		// we are done with the current node.
		// switch the current node to the closed list...
		closeList.insert( current );
		pop_heap( openList.begin( ), openList.end( ), less_f );
		openList.pop_back( );


		if( current->node == pEndNode )
		{
			startOfPath = *current;
			bPathFound = true;
			break;
		}	

		for( int i = -1; i <= 1; i++ )
		{
			for( int j = -1; j <= 1; j++ )
			{	
				if( i == 0 && j == 0 ) continue; // this is the current node, and not a successor.

				int x = current->node->X + j;
				int y = current->node->Y + i;

				// wrap around...
				if( y < 0 ) y = Game::GAME_BOARD_SIZE + y;
				if( x < 0 ) x = Game::GAME_BOARD_SIZE + x;
				if( y >= Game::GAME_BOARD_SIZE ) y = y - Game::GAME_BOARD_SIZE;
				if( x >= Game::GAME_BOARD_SIZE ) x = x - Game::GAME_BOARD_SIZE;
				// ensure we didn't fall off the game board...
				//if( x < 0 ) continue;
				//if( y < 0 ) continue;
				//if( x >= Game::GAME_BOARD_SIZE ) continue;
				//if( y >= Game::GAME_BOARD_SIZE ) continue;

				Object *pObj = retrieveObject( x, y ); // this is a disgusting hack...

				if( pObj->type == Object::OBJ_BLOCK ) continue; // can't walk through blocks...


				AStarNode<Object *> *newNode = m_Allocator.allocate( 1 );
				m_Allocator.construct( newNode, AStarNode<Object *>( pObj ) );
				//AStarNode<Object *> newNode( pObj );



				OpenCollection::iterator itrOpen = find( openList.begin( ), openList.end( ), newNode );
				ClosedCollection::iterator itrClosed;


				int g = current->g + G( newNode->node, current->node );					
				
				//newNode->g = g;
				//newNode->h = H( newNode->node, pEndNode );
				//newNode->f = newNode->g + newNode->h;
				//newNode->parent = &current;


				// already on the open list...
				if( itrOpen != openList.end( ) )
				{

					if( g < (*itrOpen)->g ) // estimate is better...
					{
						//openList.erase( (*itrOpen) );					
						(*itrOpen)->parent = current;

						assert( (*itrOpen)->parent != NULL );




						(*itrOpen)->g = g;
						(*itrOpen)->h = H( (*itrOpen)->node, pEndNode );
						(*itrOpen)->f = (*itrOpen)->g + (*itrOpen)->h;
						make_heap( openList.begin( ), openList.end( ), less_f ); // re-heapify it
					}
				}
				else if(  ( itrClosed = closeList.find( newNode ) ) != closeList.end( ) )
				{					
					if( g < (*itrClosed)->g ) // estimate is better...
					{
						(*itrClosed)->parent = current;

						assert( (*itrClosed)->parent != NULL );

						
						(*itrClosed)->h = H( (*itrClosed)->node, pEndNode );
						(*itrClosed)->g = g;
						(*itrClosed)->f = (*itrClosed)->g + (*itrClosed)->h;

						openList.push_back( *itrClosed );
						push_heap( openList.begin( ), openList.end( ), less_f ); // push onto a binary min heap...


						closeList.erase( itrClosed );
					}
				}
				else // this node is not on the open list...
				{
					newNode->g = g;
					newNode->h = H( newNode->node, pEndNode );
					newNode->f = newNode->g + newNode->h;
					newNode->parent = current;
						assert( newNode->parent != NULL );
					
					openList.push_back( newNode );
					push_heap( openList.begin( ), openList.end( ), less_f ); // push onto a binary min heap...

				}


			}
		}

	}

	return bPathFound;
}


Object *AStar::retrieveObject( unsigned int x, unsigned int y )
{
	Object *pObj = NULL;

	if( Game::pPlayer->X == x && Game::pPlayer->Y == y ) return Game::pPlayer;
	if( Game::ghosts[ 0 ]->X == x && Game::ghosts[ 0 ]->Y == y ) return Game::ghosts[ 0 ];
	if( Game::ghosts[ 1 ]->X == x && Game::ghosts[ 1 ]->Y == y ) return Game::ghosts[ 1 ];
	if( Game::ghosts[ 2 ]->X == x && Game::ghosts[ 2 ]->Y == y ) return Game::ghosts[ 2 ];
	if( Game::ghosts[ 3 ]->X == x && Game::ghosts[ 3 ]->Y == y ) return Game::ghosts[ 3 ];
	else 
		return Game::gameBoard[ y * Game::GAME_BOARD_SIZE + x ];
}


void AStar::cleanup( )
{
	OpenCollection::iterator itrOpen;

	for( itrOpen = openList.begin( ); itrOpen != openList.end( ); ++itrOpen )
	{
		m_Allocator.destroy( *itrOpen );
		m_Allocator.deallocate( *itrOpen, 1 );
	}

	ClosedCollection::iterator itrClosed;

	for( itrClosed = closeList.begin( ); itrClosed != closeList.end( ); ++itrClosed )
	{
		m_Allocator.destroy( *itrClosed );
		m_Allocator.deallocate( *itrClosed, 1 );
	}
	
	openList.clear( );
	closeList.clear( );
}

//
//bool AStar::computeShortestPath( Object *pStartNode, Object *pEndNode )
//{
//	_pEndNode = pEndNode;
//	openList.clear( );
//	closeList.clear( );
//	less_AStar_f less;
//
//	make_heap( openList.begin( ), openList.end( ), less ); // create a binary min heap
//
//	openList.push_back( buildNode( pStartNode, NULL ) );
//	push_heap( openList.begin( ), openList.end( ), less ); // push onto a binary min heap...
//
//	bPathFound = false;
//
//	while( openList.size( ) > 0 && !bPathFound )
//	{
//
//		AStarNode &current = openList.front( );
//		pop_heap( openList.begin( ), openList.end( ) );
//		openList.pop_front( );
//
//		if( current.node == pEndNode )
//		{
//			bPathFound = true;
//
//			if( shortestPath.size( ) > 0 ) shortestPath.pop( );
//
//			AStarNode *pNode = &current;
//
//			while( pNode->pParent != NULL )
//			{
//				shortestPath.push( pNode->node );
//				pNode = pNode->pParent;
//			}
//		}
//
//		for( int i = -1; i <= 1; i++ )
//		{
//			for( int j = -1; j <= 1; j++ )
//			{
//				int x = current.node->X + j;
//				int y = current.node->Y + i;
//
//				if( x < 0 ) continue;
//				if( y < 0 ) continue;
//				if( x > Game::GAME_BOARD_SIZE ) continue;
//				if( y > Game::GAME_BOARD_SIZE ) continue;
//
//				Object *pObj = Game::gameBoard[ y * Game::GAME_BOARD_SIZE + x ];
//
//				if( pObj->type == Object::OBJ_BLOCK ) continue; // can't walk through blocks...
//				if( closeList.find( pObj ) != closeList.end( ) ) continue; // its in the close list...
//
//
//				OpenCollection::iterator itr = find( openList.begin( ), openList.end( ), pObj );
//
//				if( itr == openList.end( ) )
//				{
//					openList.push_back( buildNode( pObj, &current ) );
//					push_heap( openList.begin( ), openList.end( ), less ); // push onto a binary min heap...
//				}
//				else // already on the open list
//				{
//					int g = current.g + _G(current.node, pObj);
//
//					if( g < itr->g )
//					{
//						itr->pParent = &current;
//						itr->g = g;
//						itr->h = _H( pObj, pEndNode );
//						itr->f = itr->g + itr->h;
//
//						 sort_heap( openList.begin( ), openList.end( ), less ); // create a binary min heap
//					}
//				}
//
//			}
//		}
//	}
//
//	return bPathFound;
//}